from Maix import MIC_ARRAY as mic
from Maix import GPIO
from Maix import FPIOA
import lcd,time
import math
import utime
from machine import UART,Timer
from fpioa_manager import fm



#引脚映射
#打开串口
fm.register(15, fm.fpioa.UART1_TX, force=True)
fm.register(16, fm.fpioa.UART1_RX, force=True)


#构造函数

uart_A = UART(UART.UART1, 115200, 8, 0, 0, timeout=1000, read_buf_len=4096)



#*****************************************Maike********************************#
#import maix_vision
lcd.init()
mic.init()
#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
            #sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
#mic.init(i2s_d0=23, i2s_d1=22, i2s_d2=21, i2s_d3=20, i2s_ws=19, i2s_sclk=18, sk9822_dat=24, sk9822_clk=25)
#mic.init(i2s_d0=20, i2s_d1=21, i2s_d2=15, i2s_d3=8, i2s_ws=7, i2s_sclk=6, sk9822_dat=25, sk9822_clk=24)# for maix cube

meancount = 100
mean_x =[]
meanwriteindex = 0
meanfound = False
meanlost = 0

x230 = [0]

def set_mean_buffer(buffercount):
    global meancount

    meancount = buffercount

    global mean_x
    mean_x.clear()
    for i in range(meancount):
        mean_x.append(0);

#缺省200过滤
#set_mean_buffer(200)

##正角度 10-25
x2cm_k_upper_150to75 = 3.378318#2.272727

#10度以内
x2cm_k_upper_75to0 = 2.377319


###负角度 10度
x2cm_k_lower_75to0 = 3.378318

#-10- 30
x2cm_k_lower_150to75 = 3.378318 #2.161383

x2cm_k_lower_150to50=3.378318

x2cm_x50 = 25
x2cm_x150 = 57
x2cm_x75 = 15

x2cm_x0 = 0

x2cm_Fx75 = -15
x2cm_Fx150 = -57
x2cm_Fx50 = -25

#接收设备中点到音源中心的距离,cm
chuizhichangdu = 225

def ConvertToDensity(getdata):
    if getdata > x2cm_x150:
        return 150


####正角度  25-50
    elif getdata > x2cm_x50:
       return (x2cm_k_upper_150to75 * (getdata - x2cm_x50)) + 20
####正角度 10 -25
    elif getdata > x2cm_x75:
       return (x2cm_k_upper_150to75 * (getdata - x2cm_x75)) + 15
####10度以内
    elif getdata > x2cm_x0:
        return x2cm_k_upper_75to0 *getdata



####负角度  -10度以内
    elif getdata > x2cm_Fx75:
        return x2cm_k_lower_75to0 *getdata
    ##-10 -25
    elif getdata > x2cm_Fx150:
        return (x2cm_k_lower_150to75 * (getdata - x2cm_Fx75)) - 15

    ##-25 -50
    elif getdata > x2cm_Fx50:
        return (x2cm_k_lower_150to50 * (getdata - x2cm_Fx75)) - 30



    #elif getdata <= x2cm_Fx150:
    else:
        return -150

               # th = math.atan(x/chuizhichangdu)*180 / math.pi

#根据阈值查找
def get_mic_dir(xresult):
    global meanfound
    global mean_x
    global meanwriteindex
    global meancount
    global meanlost
    global chuizhichangdu
    global x230
    global x2cm_k_upper
    global x2cm_a_upper
    global x2cm_k_lower
    global x2cm_a_lower
    global x2cm_x0


    okcount = 0

    imga = mic.get_map()    # 获取声音源分布图像
    Voice = mic.get_dir(imga)#获取声音数组
    mic.set_led(Voice,(0,0,255))

    imgb = imga.resize(180, 180)

    tempx = 0

    #th = [(230, 255)]
    th = [(120, 255)]
    #blobs = imgb.find_blobs(th, area_threshold=16, pixels_threshold=1, merge=True, x_stride=1, y_stride=1)
    blobs = imgb.find_blobs(th, area_threshold=16, pixels_threshold=1, merge=True, x_stride=1, y_stride=1)
    #print(blobs)
    if blobs:
        tempx = blobs[0].cx() - 80
        okcount = okcount + 1
    else:
        tempx = 0

    th = [(10, 255)]
    #blobs = imgb.find_blobs(th, area_threshold=16, pixels_threshold=1, merge=True, x_stride=1, y_stride=1)
    blobs = imgb.find_blobs(th, area_threshold=16, pixels_threshold=1, merge=True, x_stride=1, y_stride=1)

    if blobs:
        tempx = tempx + blobs[0].cx() - 80
        okcount = okcount + 1


    if okcount > 0:
        xresult[0] = tempx / okcount
        return True
    else:
        return False


#卡尔曼滤波
def Kalman_Filter():
    global Flag
    e = 12
    E = get_mic_dir(x230)
    while True:
        Z = get_mic_dir(x230)     #采样
        K = e / (e + E_measure)   #计算Kerman增益
        E = E + K * (Z - E)       # 迭代目标值
        e = (1 - K) * e           # 更新估计误差
     #   print(E)
        if K <= 0.012 and Flag == 0:
            Flag = 1
        if K <= 0.005:             #成功判断
            break
    return E



def test3():
    global meanfound
    global mean_x
    global meanwriteindex
    global meancount
    global meanlost
    global chuizhichangdu
    global x230
    global x2cm_k_upper
    global x2cm_a_upper
    global x2cm_k_lower
    global x2cm_a_lower
    global x2cm_x0

    lcd.clear()
    lcd.draw_string(10, 10, "Test3:", lcd.YELLOW, lcd.BLACK)

    #设置过滤缓冲300
    set_mean_buffer(50)

    meanwriteindex = 0
    meanfound = False


    while True:
        if get_mic_dir(x230) == True:
            meanlost = 0
            mean_x[meanwriteindex] = x230[0]
            meanwriteindex = meanwriteindex + 1
            if meanfound == False:#第一次满
                if meanwriteindex >= meancount:
                    meanfound = True
            meanwriteindex = meanwriteindex % meancount


            #如果满了就可以计算了
            if meanfound == True:
                allv = 0.0
                #print(mean_x)
                max_num = mean_x[0]
                min_num = mean_x[0]
                for i in range(meancount):
                    if mean_x[i] >  max_num:
                        max_num = mean_x[i]
                    if mean_x[i] < min_num:
                        min_num = mean_x[i]

                    allv = allv + mean_x[i]
                allv = allv - max_num - min_num
                allv = (allv / (meancount-2))


                x = ConvertToDensity(allv)


                th = math.atan(x/chuizhichangdu)*180 / math.pi


                if(int(th) == 0):
                    servo=90
                elif(th < 5 and th >=0):  #向右补偿
                    servo=90-th+4
                elif(th < 10 and th >=5):  #向右补偿
                    servo=90-th+3
                elif(th < 20 and th >=10):  #向右补偿
                    servo=90-th+5
                elif(th < 25 and th >=20):  #向右补偿
                    servo=90-th+8         #非线性
                elif(th < 30 and th >=25):  #向右补偿
                    servo=90-th+6
                elif(th < 35 and th >=30):  #向右补偿    208 222 246
                    servo=90-th+3


                elif(th > -3 and th <=0):    #向左补偿
                    servo=90-th+2
                elif(th > -5 and th <=-3):    #向左补偿
                    servo=90-th+2
                elif(th > -10 and th <=-5):    #向左补偿
                    servo=90-th+2
                elif(th > -15 and th <=-10):    #向左补偿
                    servo=90-th+4
                elif(th > -20 and th <=-15):    #向左补偿
                    servo=90-th+4
                elif(th > -25 and th <=-20):    #向左补偿
                    servo=90-th+2
                elif(th > -30 and th <=-25):    #向左补偿
                    servo=90-th+2
                elif(th > -40 and th <=-30):    #向左补偿
                    servo=90-th-2

                else:
                    servo=90-th+6

                if(allv > 55 and allv <=62):
                    servo=servo-allv%10+3
                elif(allv > 62 and allv < 70):
                    servo=servo-allv%10+6
                elif(allv >=70 ):
                    servo=servo-68%10+4
                elif(allv < -52):
                    servo=servo-allv%10+8

                Angle=int(servo)

                uart_A.write(bytearray([Angle])) #发送角度信息

                r = math.sqrt(x*x + chuizhichangdu*chuizhichangdu)-20

                print("{" + str(allv) + ";" + str(Angle)+";" + str(r) + ";" + str(th) + "}")
                lcd.draw_string(10, 30, "rawx:%0.1f; x:%0.1f        "% (allv ,x), lcd.YELLOW, lcd.BLACK)
                lcd.draw_string(10, 60, "L:%0.1f; Theta:%0.1f        "%(r, servo), lcd.YELLOW, lcd.BLACK)

                mic.deinit()


            else:
                print("WAIT FULL")


        else:#如果是无效帧
            meanlost = meanlost + 1
            if meanlost > 10:
                meanfound = False





def test4():
    global meanfound
    global mean_x
    global meanwriteindex
    global meancount
    global meanlost
    global chuizhichangdu
    global x230
    global x2cm_k_upper
    global x2cm_a_upper
    global x2cm_k_lower
    global x2cm_a_lower
    global x2cm_x0

    lcd.clear()
    lcd.draw_string(10, 10, "Test3:", lcd.YELLOW, lcd.BLACK)

    #设置过滤缓冲300
    set_mean_buffer(20)

    meanwriteindex = 0
    meanfound = False


    while True:
        if get_mic_dir(x230) == True:
            meanlost = 0
            mean_x[meanwriteindex] = x230[0]
            meanwriteindex = meanwriteindex + 1
            if meanfound == False:#第一次满
                if meanwriteindex >= meancount:
                    meanfound = True
            meanwriteindex = meanwriteindex % meancount


            #如果满了就可以计算了
            if meanfound == True:
                allv = 0.0
                #print(mean_x)
                max_num = mean_x[0]
                min_num = mean_x[0]
                for i in range(meancount):
                    if mean_x[i] >  max_num:
                        max_num = mean_x[i]
                    if mean_x[i] < min_num:
                        min_num = mean_x[i]

                    allv = allv + mean_x[i]
                allv = allv - max_num - min_num
                allv = (allv / (meancount-2))


                x = ConvertToDensity(allv)


                th = math.atan(x/chuizhichangdu)*180 / math.pi


                if(int(th) == 0):
                    servo=90
                elif(th < 5 and th >=0):  #向右补偿
                    servo=90-th+4
                elif(th < 10 and th >=5):  #向右补偿
                    servo=90-th+3
                elif(th < 20 and th >=10):  #向右补偿
                    servo=90-th+5
                elif(th < 25 and th >=20):  #向右补偿
                    servo=90-th+8         #非线性
                elif(th < 30 and th >=25):  #向右补偿
                    servo=90-th+6
                elif(th < 35 and th >=30):  #向右补偿    208 222 246
                    servo=90-th+3


                elif(th > -3 and th <=0):    #向左补偿
                    servo=90-th+2
                elif(th > -5 and th <=-3):    #向左补偿
                    servo=90-th+2
                elif(th > -10 and th <=-5):    #向左补偿
                    servo=90-th+2
                elif(th > -15 and th <=-10):    #向左补偿
                    servo=90-th+4
                elif(th > -20 and th <=-15):    #向左补偿
                    servo=90-th+4
                elif(th > -25 and th <=-20):    #向左补偿
                    servo=90-th+2
                elif(th > -30 and th <=-25):    #向左补偿
                    servo=90-th+2
                elif(th > -40 and th <=-30):    #向左补偿
                    servo=90-th-2

                else:
                    servo=90-th+6

                if(allv > 55 and allv <=62):
                    servo=servo-allv%10+3
                elif(allv > 62 and allv < 70):
                    servo=servo-allv%10+6
                elif(allv >=70 ):
                    servo=servo-68%10+4
                elif(allv < -52):
                    servo=servo-allv%10+8

                Angle=int(servo)

                uart_A.write(bytearray([Angle])) #发送角度信息

                r = math.sqrt(x*x + chuizhichangdu*chuizhichangdu)-20

                print("{" + str(allv) + ";" + str(Angle)+";" + str(r) + ";" + str(th) + "}")
                #lcd.draw_string(10, 30, "rawx:%0.1f; x:%0.1f        "% (allv ,x), lcd.YELLOW, lcd.BLACK)
                #lcd.draw_string(10, 60, "L:%0.1f; Theta:%0.1f        "%(r, th), lcd.YELLOW, lcd.BLACK)



            else:
                print("WAIT FULL")


        else:#如果是无效帧
            meanlost = meanlost + 1
            if meanlost > 10:
                meanfound = False



#显示液晶
lcd.clear()
lcd.draw_string(100, 100, "select test:", lcd.YELLOW, lcd.BLACK)

#mic.set_led([0,0,0,0,0,0,0,0,0,0,0,0],(0,0,0))

while True:

    test3()
    #test4()
    #time.sleep_ms(5000)


mic.deinit()
